
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs_text_queue.h
  * @author     baiyang
  * @date       2022-11-6
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "gcs_mavlink.h"

#include <rtthread.h>

#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
#define  GCS_STATUS_CAPACITY     7
#else
#define  GCS_STATUS_CAPACITY     20
#endif
/*----------------------------------typedef-----------------------------------*/
struct statustext_t {
    mavlink_statustext_t    msg;
    uint16_t                entry_created_ms;
    uint8_t                 bitmask;
};

typedef struct StatusTextQueue* status_text_queue_t;
struct StatusTextQueue {
    struct rt_mutex         mutex;
    uint32_t                last_prune_ms;

    struct statustext_t     _buffer[GCS_STATUS_CAPACITY];
    uint16_t                _size;  // total buffer size
    uint16_t                _count; // number in buffer now
    uint16_t                _head;  // first element
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static inline void status_text_queue_ctor(status_text_queue_t queue) {
    rt_mutex_init(&queue->mutex, "text_que", RT_IPC_FLAG_PRIO);
    queue->_count = 0;
    queue->_head  = 0;
    queue->_size  = GCS_STATUS_CAPACITY;
}

// return total number of objects
static inline uint16_t status_text_queue_size(status_text_queue_t queue) {
    return queue->_size;
}

// return number of objects available to be read
static inline uint16_t status_text_queue_available(status_text_queue_t queue) {
    return queue->_count;
}

// return number of objects that could be written
static inline uint16_t status_text_queue_space(status_text_queue_t queue) {
    return queue->_size - queue->_count;
}

// true is available() == 0
static inline bool status_text_queue_is_empty(status_text_queue_t queue) {
    return queue->_count == 0;
}

// push one object
static inline bool status_text_queue_push(status_text_queue_t queue, struct statustext_t* object) {
    if (status_text_queue_space(queue) == 0) {
        return false;
    }
    queue->_buffer[(queue->_head+queue->_count)%queue->_size] = *object;
    queue->_count++;
    return true;
}

/*
  throw away an object
 */
static inline bool status_text_queue_pop2(status_text_queue_t queue) {
    if (status_text_queue_is_empty(queue)) {
        return false;
    }
    queue->_head = (queue->_head+1) % queue->_size;
    queue->_count--;
    return true;
}

// Discards the buffer content, emptying it.
static inline void status_text_queue_clear(status_text_queue_t queue) {
    queue->_head = queue->_count = 0;
}

/*
  pop earliest object off the queue
 */
static inline bool status_text_queue_pop(status_text_queue_t queue, struct statustext_t* object) {
    if (status_text_queue_is_empty(queue)) {
        return false;
    }
    *object = queue->_buffer[queue->_head];
    return status_text_queue_pop2(queue);
}

/*
 * push_force() is semantically equivalent to:
 *   if (!push(t)) { pop(); push(t); }
 */
static inline bool status_text_queue_push_force(status_text_queue_t queue, struct statustext_t* object) {
    if (status_text_queue_space(queue) == 0) {
        (void)(status_text_queue_pop2(queue));
    }
    return status_text_queue_push(queue, object);
}

/*
  remove the Nth element from the array. First element is zero
 */
static inline bool status_text_queue_remove(status_text_queue_t queue, uint16_t n) {
    if (n >= queue->_count) {
        return false;
    }
    if (n == queue->_count-1) {
        // remove last element
        queue->_count--;
        return true;
    }
    if (n == 0) {
        // remove first element
        return status_text_queue_pop2(queue);
    }
    // take advantage of the [] operator for simple shift of the array elements
    for (uint16_t i=n; i<queue->_count-1; i++) {
        queue->_buffer[i] = queue->_buffer[i+1];
    }
    queue->_count--;
    return true;
}

static inline void status_text_queue_prune(status_text_queue_t queue)
{
    // consider pruning the statustext queue of ancient entries
    const uint32_t now_ms = time_millis();
    if (now_ms - queue->last_prune_ms < 1000) {
        return;
    }
    queue->last_prune_ms = now_ms;

    const uint16_t now16_ms = time_millis16();
    for (uint8_t idx=0; idx<status_text_queue_available(queue); ) {
        const struct statustext_t *statustext = &queue->_buffer[idx];

        // be wary of integer promotion here
        const uint16_t age = now16_ms - statustext->entry_created_ms;
        if (age > 5000) {
            // too old.  Purge it.
            status_text_queue_remove(queue, idx);
            continue;
        }
        // this is a queue.  If this one wasn't too old then the next
        // one isn't either.
        break;
    }
}

static inline rt_err_t status_text_queue_take_blocking(status_text_queue_t queue)
{
    return rt_mutex_take(&queue->mutex, RT_WAITING_FOREVER);
}

static inline rt_err_t status_text_queue_give(status_text_queue_t queue)
{
    return rt_mutex_release(&queue->mutex);
}

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



